//
// Created by lqk on 2022/2/21.
//

#ifndef QUADCEREBELLUM_STATESTRUCTS_H
#define QUADCEREBELLUM_STATESTRUCTS_H

#include "Eigen/Eigen"

struct OrientationState{
    OrientationState(){Zero();}
    void Zero(){
        m_rotation.setZero();
        m_rpy.setZero();
        m_quat.setIdentity();
    }
    Eigen::Quaternionf m_quat;
    Eigen::Vector3f m_rpy;
    Eigen::Matrix3f m_rotation;
};

struct PosVelState{
    PosVelState(){Zero();}
    void Zero(){
        m_position.setZero();
        m_velWorld.setZero();
        m_velBody.setZero();
        m_omegaWorld.setZero();
        m_omegaBody.setZero();
    }
    Eigen::Vector3f m_position;
    Eigen::Vector3f m_velBody;
    Eigen::Vector3f m_velWorld;
    Eigen::Vector3f m_omegaBody;
    Eigen::Vector3f m_omegaWorld;
};

struct EstimatedState{
    EstimatedState(){Zero();}
    void Zero(){
        m_position.setZero();
        m_velWorld.setZero();
        m_velBody.setZero();
        m_omegaWorld.setZero();
        m_omegaBody.setZero();
        m_rotation.setZero();
        m_rpy.setZero();
        m_quat.setIdentity();
    }
    Eigen::Quaternionf m_quat;
    Eigen::Vector3f m_rpy;
    Eigen::Matrix3f m_rotation;
    Eigen::Vector3f m_position;
    Eigen::Vector3f m_velBody;
    Eigen::Vector3f m_velWorld;
    Eigen::Vector3f m_omegaBody;
    Eigen::Vector3f m_omegaWorld;
};

#endif //QUADCEREBELLUM_STATESTRUCTS_H
